#include "RM3508.h"
#include "string.h"
#include "CAN_Encoder.h"
#include "spi_bsp.h"

/*********************************************************************************
 速度环PID设置
*********************************************************************************/
M3508_PID M3508_Speed_Pid[8] =
	{
		{.Kp = 8, .Ki = 0.1f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 100, .DeadBand = 1},	// ID = 1
		{.Kp = 8, .Ki = 0.f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 2000, .DeadBand = 1},	// ID = 2
		{.Kp = 8, .Ki = 0.f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 1000, .DeadBand = 1},	// ID = 3
		{.Kp = 15, .Ki = 0.1f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 1000, .DeadBand = 1}, // ID = 4
		{.Kp = 8, .Ki = 0.f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 1000, .DeadBand = 1},	// ID = 5
		{.Kp = 8, .Ki = 0.f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 1000, .DeadBand = 1},	// ID = 6
		{.Kp = 8, .Ki = 0.f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 1000, .DeadBand = 1},	// ID = 7
		{.Kp = 8, .Ki = 0.f, .Kd = 5, .Max = 16000, .Min = -16000, .IntegralLimit = 1000, .DeadBand = 1},	// ID = 8
};
/*********************************************************************************
 位置环PID设置
*********************************************************************************/
M3508_PID M3508_Pos_Pid[8] =
	{
		{.Kp = 100, .Ki = 0.3, .Kd = 200, .Max = 1000, .Min = -1000, .IntegralLimit = 1000, .I_Start_Err = 10, .DeadBand = 0},		// ID = 1
		{.Kp = 4, .Ki = 0.005f, .Kd = 20, .Max = 2400, .Min = -2400, .IntegralLimit = 1000, .I_Start_Err = 50, .DeadBand = 0},		// ID = 2
		{.Kp = 0.4, .Ki = 0.003, .Kd = 0.3, .Max = 14000, .Min = -14000, .IntegralLimit = 20000, .DeadBand = 10},					// ID = 3
		{.Kp = 0.2, .Ki = 0.003, .Kd = 0.2, .Max = 600, .Min = -600, .IntegralLimit = 20000, .I_Start_Err = 1000, .DeadBand = 2}, // ID = 4
		{.Kp = 0.4, .Ki = 0.003, .Kd = 0.3, .Max = 14000, .Min = -14000, .IntegralLimit = 20000, .DeadBand = 10},					// ID = 5
		{.Kp = 0.4, .Ki = 0.003, .Kd = 0.3, .Max = 14000, .Min = -14000, .IntegralLimit = 20000, .DeadBand = 10},					// ID = 6
		{.Kp = 0.4, .Ki = 0.003, .Kd = 0.3, .Max = 14000, .Min = -14000, .IntegralLimit = 20000, .DeadBand = 10},					// ID = 7
		{.Kp = 0.4, .Ki = 0.003, .Kd = 0.3, .Max = 14000, .Min = -14000, .IntegralLimit = 20000, .DeadBand = 10},					// ID = 8
};

// 电机减速比数组
const float RM3508_Reduction_Ratio[8] = {1, 1, 1, 1, 1, 1, 1, 1};

// 用于存储电机反馈的全局变量
uint8_t RM3508_Feedback_Buf[8][7];	 // 电机反馈值(全局变量)
int RM3508_Pos[8];					 // 每一个元素对应一个ID的电机的信息
uint8_t RM3508_Sendbuf1[8] = {0};	 // CAN发送数据
uint8_t RM3508_Sendbuf2[8] = {0};	 // CAN发送数据
static int32_t RM3508_base[8] = {0}; // 用来标记已经转过的编码器线数，一圈8192
uint8_t RM3508_error = 0;			 // 错误类型
uint8_t RM3508_Init_Flag[8];

RM3508_State_t RM3508, RM3508_backup; // 电机状态

void RM3508_Stop()
{
	for (int i = 0; i < 8; i++)
	{
		RM3508_backup.Mode[i] = RM3508.Mode[i];
		RM3508_backup.Target[i] = RM3508.Target[i];
		RM3508_Sendbuf1[i] = 0;
	}

	RM3508_CAN_Send_Data(&hcan1); // 因为堵死在master，所以必须手动发送
}
void RM3508_resume()
{
	for (int i = 0; i < 8; i++)
	{
		RM3508.Mode[i] = RM3508_backup.Mode[i];
		RM3508.Target[i] = RM3508_backup.Target[i];
	}
	// master不再阻塞，所以无需手动发送
}
/*********************************************************************************
 *@  name      : RM3508_Set
 *@  function  : RM3508模式&目标值设置
 *@  input     : 模式，目标值，电机id
 *********************************************************************************/
void RM3508_Set(uint8_t mode, float target, uint8_t ID)
{
	RM3508.Mode[ID - 1] = mode;
	RM3508.Target[ID - 1] = target;
}
/*********************************************************************************
 *@  name      : RM3508_PID_Run
 *@  function  : RM3508PID运行
 *@  input     : 电机id
 *********************************************************************************/
void RM3508_PID_Run(uint8_t ID)
{
	switch (RM3508.Mode[ID - 1])
	{
	case mode_Close:
		RM3508_Set_I(0, ID);
		break;
	case mode_I:
		RM3508_Set_I(RM3508.Target[ID - 1], ID);
		break;
	case mode_Speed:
		RM3508_Set_Speed(RM3508.Target[ID - 1], ID);
		break;
	case mode_Pos:
		RM3508_Set_Pos(RM3508.Target[ID - 1], ID);
		break;
	default:
		Error_RM3508(3);
		break;
	}
}
/*********************************************************************************
 *@  name      : RM3508_Set_I
 *@  function  : RM3508电机电流设置
 *@  input     : 目标电流，电机id
 *@  output    : 成功返回0，失败返回1
 *********************************************************************************/
void RM3508_Set_I(int target_i, uint8_t motor_id)
{
	if (motor_id < 1 || motor_id > 8)
		Error_RM3508(1);

	if (motor_id <= 4) // 前四个ID对应的sendID标识符是0x200
	{
		RM3508_Sendbuf1[2 * motor_id - 2] = target_i >> 8;	   // 电流值高8位
		RM3508_Sendbuf1[2 * motor_id - 1] = target_i & 0x00ff; // 电流值低8位

		// 操作系统中统一发送can消息，CAN1控制1-4号电机，CAN2控制5-8号电机

#if 0
		RM3508_CAN_Send_Data(&hcan1, RM3508_Sendbuf1, 0x200, 8);
#endif
	}
	else // 后四个ID对应的sendID标识符是0x1ff
	{
		motor_id -= 4;

		RM3508_Sendbuf2[2 * motor_id - 2] = target_i >> 8;	   // 电流值高8位
		RM3508_Sendbuf2[2 * motor_id - 1] = target_i & 0x00ff; // 电流值低8位
#if 0
		RM3508_CAN_Send_Data(&hcan2, RM3508_Sendbuf2, 0x1ff, 8);
#endif
	}
}
/********************************************************************************
 *@  name      : RM3508_CAN_Send_Data
 *@  function  : CAN发送数据
 *@  input     : 哪条CAN线，要发送的数据，发送的仲裁段ID，数据长度
 *@  output    : 成功返回0，失败返回2
 *********************************************************************************/
void RM3508_CAN_Send_Data(CAN_HandleTypeDef *hcan)
{
	HAL_StatusTypeDef HAL_RetVal = HAL_ERROR;
	CAN_TxHeaderTypeDef TxMessage;
	uint32_t TxMailboxX = CAN_TX_MAILBOX0; // CAN发送邮箱

	TxMessage.IDE = CAN_ID_STD;	  // 标准帧,CAN_ID_EXT扩展帧;
	TxMessage.RTR = CAN_RTR_DATA; // 数据帧,CAN_RTR_REMOTE遥控帧
	TxMessage.DLC = 8;

	while (!HAL_CAN_GetTxMailboxesFreeLevel(hcan)) // 等待空邮箱
		;
	if ((hcan->Instance->TSR & CAN_TSR_TME0) != RESET) // 检查发送邮箱状态
		TxMailboxX = CAN_TX_MAILBOX0;
	else if ((hcan->Instance->TSR & CAN_TSR_TME1) != RESET)
		TxMailboxX = CAN_TX_MAILBOX1;
	else if ((hcan->Instance->TSR & CAN_TSR_TME2) != RESET)
		TxMailboxX = CAN_TX_MAILBOX2;

	if (hcan == &hcan1) // CAN1控制1-4号电机
	{
		TxMessage.StdId = 0x200;
		HAL_RetVal = HAL_CAN_AddTxMessage(hcan, &TxMessage, RM3508_Sendbuf1, (uint32_t *)TxMailboxX);
	}
	else if (hcan == &hcan2) // CAN2控制5-8号电机
	{
		TxMessage.StdId = 0x1ff;
		HAL_RetVal = HAL_CAN_AddTxMessage(hcan, &TxMessage, RM3508_Sendbuf2, (uint32_t *)TxMailboxX);
	}

	if (HAL_RetVal != HAL_OK)
		Error_RM3508(2);
}
/********************************************************************************
 *@  name      : RM3508_Set_Speed
 *@  function  : RM3508速度设置
 *@  input     : 目标速度（-15000~15000都可接受），电机id
 *@  output    : 无
 ********************************************************************************/
void RM3508_Set_Speed(int goal_speed, int ID)
{
	uint8_t id = ID - 1;
	M3508_Speed_Pid[id].Target = goal_speed;
	M3508_Speed_Pid[id].NowIS = RM3508_Get_Speed(ID);
	M3508_Speed_Pid[id].Err_last = M3508_Speed_Pid[id].Err;
	M3508_Speed_Pid[id].Err = M3508_Speed_Pid[id].Target - M3508_Speed_Pid[id].NowIS;

	if (RM3508_ABS(M3508_Speed_Pid[id].Err) < M3508_Speed_Pid[id].DeadBand)
	{
		M3508_Speed_Pid[id].Err = 0;
		// M3508_Speed_Pid[id].Err_sum = 0;
	}

	M3508_Speed_Pid[id].Err_sum += M3508_Speed_Pid[id].Err;
	M3508_Speed_Pid[id].Err_sum = CLAMP(M3508_Speed_Pid[id].Err_sum, -M3508_Speed_Pid[id].IntegralLimit, M3508_Speed_Pid[id].IntegralLimit);

	M3508_Speed_Pid[id].P_Out = M3508_Speed_Pid[id].Kp * M3508_Speed_Pid[id].Err;
	M3508_Speed_Pid[id].I_Out = M3508_Speed_Pid[id].Ki * M3508_Speed_Pid[id].Err_sum;
	M3508_Speed_Pid[id].D_Out = M3508_Speed_Pid[id].Kd * (M3508_Speed_Pid[id].Err - M3508_Speed_Pid[id].Err_last);

	M3508_Speed_Pid[id].PID_Out = M3508_Speed_Pid[id].P_Out + M3508_Speed_Pid[id].I_Out + M3508_Speed_Pid[id].D_Out;
	M3508_Speed_Pid[id].PID_Out = CLAMP(M3508_Speed_Pid[id].PID_Out, M3508_Speed_Pid[id].Min, M3508_Speed_Pid[id].Max);

	RM3508_Set_I(M3508_Speed_Pid[id].PID_Out, ID);
}
/********************************************************************************
 *@  name      : RM3508_Set_Pos
 *@  function  : rm3508位置设置
 *@  input     : 目标角度（任意角度），电机id
 *@  output    : 无
 ********************************************************************************/
void RM3508_Set_Pos(float angle, int ID)
{
	uint8_t id = ID - 1;
	float goal_cnt;

	if (id >= 2 && id <= 7)
	{
		goal_cnt = RM3508_Ang2Cnt(angle, ID); // 该函数为电机角度转cnt
	}
	else if (id == 0)
	{
		goal_cnt = angle;
	}
	else if (id == 1)
	{
		goal_cnt = -angle;
	}

	M3508_Pos_Pid[id].Target = goal_cnt;
	M3508_Pos_Pid[id].NowIS = RM3508_Get_Pos(ID);
	M3508_Pos_Pid[id].Err_last = M3508_Pos_Pid[id].Err;
	M3508_Pos_Pid[id].Err = DEADBAND(goal_cnt - M3508_Pos_Pid[id].NowIS, M3508_Pos_Pid[id].DeadBand);

	if (fabsf(M3508_Pos_Pid[id].Err) < M3508_Pos_Pid[id].I_Start_Err)
		M3508_Pos_Pid[id].Err_sum += M3508_Pos_Pid[id].Err;
	else
		M3508_Pos_Pid[id].Err_sum = 0;

	M3508_Pos_Pid[id].P_Out = M3508_Pos_Pid[id].Kp * M3508_Pos_Pid[id].Err;
	M3508_Pos_Pid[id].I_Out = LIMIT(M3508_Pos_Pid[id].Ki * M3508_Pos_Pid[id].Err_sum, M3508_Pos_Pid[id].IntegralLimit);
	M3508_Pos_Pid[id].D_Out = M3508_Pos_Pid[id].Kd * (M3508_Pos_Pid[id].Err - M3508_Pos_Pid[id].Err_last);

	M3508_Pos_Pid[id].PID_Out = LIMIT(M3508_Pos_Pid[id].P_Out + M3508_Pos_Pid[id].I_Out + M3508_Pos_Pid[id].D_Out, M3508_Pos_Pid[id].Max);

	RM3508_Set_Speed(M3508_Pos_Pid[id].PID_Out, ID);
}
/*********************************************************************************
 *@  name      : RM3508_Get_Feedback
 *@  function  : 获取RM3508电机的反馈并存入全局变量RM3508_Feedback_Buf[8][7];
 *@  input     : message_id,message数组指针
 *@  output    : 无
 *********************************************************************************/
void RM3508_Get_Feedback(uint32_t std_id, uint8_t *data_p)
{
	int i;
	for (i = 1; i < 9; i++)
	{
		if (std_id == 0x200 + i)
		{
			memcpy(RM3508_Feedback_Buf[i - 1], data_p, 7);
			RM3508_Pos_Rec(i);
			return;
		}
	}
}
/*********************************************************************************
 *@  name      : RM3508_Get_Torque
 *@  function  : 获取RM3508电机的实际转矩信息
 *@  input     : 电机id号
 *@  output    : 对应id电机的转矩,读取失败返回0
 *********************************************************************************/
int RM3508_Get_Torque(uint8_t motor_id)
{
	int torque = 0;
	if (RM3508_Feedback_Buf[motor_id - 1][2] >> 7 == 1)
		torque = -(0xffff - ((RM3508_Feedback_Buf[motor_id - 1][4] << 8) + RM3508_Feedback_Buf[motor_id - 1][5]));
	else
		torque = (RM3508_Feedback_Buf[motor_id - 1][4] << 8) + RM3508_Feedback_Buf[motor_id - 1][5];
	return torque;
}
/*********************************************************************************
 *@  name      : RM3508_Get_Speed
 *@  function  : 获取RM3508电机的反馈的速度信息
 *@  input     : 电机id号
 *@  output    : 对应id电机的速度,读取失败返回0
 *********************************************************************************/
int RM3508_Get_Speed(uint8_t motor_id)
{
	int speed = 0;
	if (RM3508_Feedback_Buf[motor_id - 1][2] >> 7 == 1)
		speed = -(0xffff - ((RM3508_Feedback_Buf[motor_id - 1][2] << 8) + RM3508_Feedback_Buf[motor_id - 1][3]));
	else
		speed = (RM3508_Feedback_Buf[motor_id - 1][2] << 8) + RM3508_Feedback_Buf[motor_id - 1][3];
	return speed;
}
/*********************************************************************************
 *@  name      : RM3508_Get_Pos
 *@  function  : 获取RM3508电机当前的位置信息
 *@  input     : 电机id号
 *@  output    : 对应id电机的位置，编码器的CNT值
 *********************************************************************************/
int RM3508_Get_Pos(uint8_t motor_id)
{
	//	if(motor_id == 1)
	//		return CAN_Encoder_Pos_Back[1];
	//	else if(motor_id ==2)
	//		return CAN_Encoder_Pos_Back[2];
	//	else
	return RM3508_Pos[motor_id - 1];
}
/*********************************************************************************
 *@  name      : RM3508_Get_Temp
 *@  function  : 获取RM3508电机当前的温度信息
 *@  input     : 电机id号
 *@  output    : 对应id电机的温度值
 *********************************************************************************/
uint8_t RM3508_Get_Temp(uint8_t id)
{

	return RM3508_Feedback_Buf[id - 1][6];
}
/*********************************************************************************
 *@  name      : RM3508_Pos_Rec
 *@  function  : 获取RM3508电机的反馈的位置信息  //累积路程
 *@  input     : 电机id号
 *@  output    : 对应id电机的位置信息,读取失败返回-1
 *********************************************************************************/

static void RM3508_Pos_Rec(uint8_t motor_id)
{
	int id = motor_id - 1;
	int32_t RM3508_tmp[8];
	static int32_t RM3508tmp_pre[8] = {0};

	RM3508_tmp[id] = (RM3508_Feedback_Buf[id][0] << 8) + RM3508_Feedback_Buf[id][1];
	if (RM3508_Init_Flag[id] == 0)
	{
		RM3508_Init_Flag[id] = 1;
		RM3508_base[id] = -RM3508_tmp[id];
	}
	else
	{
		if (RM3508_tmp[id] - RM3508tmp_pre[id] > 4095) // 转过8191到0时记录圈数
			RM3508_base[id] -= 8191;
		else if (RM3508_tmp[id] - RM3508tmp_pre[id] < -4095)
			RM3508_base[id] += 8191;
	}
	RM3508tmp_pre[id] = RM3508_tmp[id];
//	if (id ==3)
//	{
//		RM3508_Pos[id] = tmp[1];
//	}
//	else 
//	{
		RM3508_Pos[id] = RM3508_base[id] + RM3508_tmp[id];
//	}
}
/********************************************************************************
 *@  name      : RM3508_Ang2Cnt
 *@  function  : 角度转换为实际电机应该转动的位置数 //未经减速箱的转轴转一圈数值为8192
 *@  input     : 目标角度（任意角度），电机id  //id不同，减速比不同
 *@  output    : 电机位置
 ********************************************************************************/
int RM3508_Ang2Cnt(float angle, int ID)
{

	int cnt;
	cnt = (int)(RM3508_CNT_PER_ROUND_OUT(ID) * angle / 360);
	return cnt;
}
/********************************************************************************
 *@  name      : RM3508_Cnt2Ang
 *@  function  : 电机位置转换为角度 //未经减速箱的转轴转一圈数值为8192
 *@  input     : 电机位置，电机id  //id不同，减速比不同
 *@  output    : 电机转过的角度
 ********************************************************************************/
float RM3508_Cnt2Ang(int32_t cnt, int ID)
{
	float angled;
	angled = (float)((cnt * 360.0) / RM3508_CNT_PER_ROUND_OUT(ID));
	return angled;
}

/********************************************************************************
 *@  name      : RM3508_Set_NowPos
 *@  function  : 将M3508电机的当前值设置为任意位置
 *@  input     : 电机id，当前位置设置为多少 //单位是编码器线数
 *@  output    : void
 ********************************************************************************/
void RM3508_Set_NowPos(uint8_t ID, int32_t Pos_Angle)
{
	uint8_t id;
	id = ID - 1;
	RM3508_base[id] = Pos_Angle;
}
/*
********************************************************************************
*@  name      : Error_RM3508
*@  function  : RM3508错误
*@  input     : error   错误号
********************************************************************************
*/
static void Error_RM3508(uint8_t error)
{
	RM3508_error = error;
	if (error == 1 || error == 2 || error == 3)
	{
		while (1)
			; // 行不行啊 细狗
	}
	// RM3508_error = 1 ：输入电机ID错误
	// RM3508_error = 2 ：CAN发送失败
	// RM3508_error = 3 ：输入电机模式错误
}
